#include "angleSolver.hpp"
using namespace cv;
using namespace std;

// int main( int argc, const char** argv )
// {

//     angleSolver angle_solver;
//     double alpha = angle_solver.get_ALPHA();
//     double thet = angle_solver.get_THET();
//     cout << "alpha is: "<<alpha*180/M_PI<<endl;
//     cout << "thet is: "<<thet*180/M_PI<<endl;

// }

angleSolver::angleSolver()
{
    pitch = 0;
    yaw = 0;
}

int angleSolver::solve()
{
    double distance = sqrt(enemy_pos.x * enemy_pos.x + enemy_pos.y * enemy_pos.y);
    double bullet_t = distance/bullet_v;
    enemy_pos.x +=enemy_v.x*bullet_t;
    enemy_pos.y +=enemy_v.y*bullet_t;
    enemy_pos.z +=enemy_v.z*bullet_t;
    distance = sqrt(enemy_pos.x * enemy_pos.x + enemy_pos.y * enemy_pos.y);
    double tempPitch = calculatePitch(distance);
    double tempYaw = calculateYaw();
    if (tempPitch == INFINITY)
    {
        return -1;
    }
    pitch = tempPitch;
    yaw = tempYaw;
    return 0;
}

double angleSolver::getPitch()
{
    return pitch;
}

double angleSolver::getYaw()
{
    return yaw;
}

double angleSolver::solveQuadratic(double a, double b, double c, char flag)
{
    double delta = b * b - 4 * a * c;
    if (delta < 0)
    {
        return INFINITY;
    }
    if (flag == '+')
    {
        return (-b + sqrt(delta)) / (2 * a);
    }
    else if (flag == '-')
    {
        return (-b - sqrt(delta)) / (2 * a);
    }

    cout << "wrong flag";
    return INFINITY;
}

double angleSolver::calculatePitch(double distance)
{
    double z = enemy_pos.z;
    double a = distance * distance + z * z;
    double b = (z * g / bullet_v / bullet_v - 1) * distance * distance;
    double c = pow(g * distance * distance / 2 / bullet_v / bullet_v, 2);
    double m = solveQuadratic(a, b, c, '+');
    if (m > 1)
    {
        m = solveQuadratic(a, b, c, '-');
    }

    if (m == INFINITY || m < 0)
    {
        return INFINITY;
    }
    double cos = sqrt(m);
    double t = distance / bullet_v / cos;
    double flag = bullet_v * sqrt(1 - cos * cos) * t - 0.5 * g * t * t - z;
    double tempPitch = acos(cos);
    if (abs(flag) > 0.01) // 判断pitch角度为正还是为副
    {
        tempPitch = -tempPitch;
    }

    return tempPitch;
}
double angleSolver::calculateYaw()
{
    double tempYaw;
    double y = enemy_pos.y;
    double x = enemy_pos.x;
    if (y == 0 && x > 0)
    {
        tempYaw = M_PI / 2;
    }
    else if (y == 0 && x < 0)
    {
        tempYaw = -M_PI / 2;
    }
    else if (y == 0 && x == 0)
    {
        tempYaw = 0;
    }
    else
    {
        tempYaw = atan(x / y);
    }
    return tempYaw;
}
void angleSolver::test()
{
    // double ans = solveQuadratic(1,-2,1);
    // cout<<ans<<endl;

    // ans = solveQuadratic(1, 0, 1);
    // cout << ans << endl;
}

// void angleSolver::setEnemyPos(Vector3d pos)
// {
//     enemy_pos = pos;
// };
void angleSolver::setEnemyPos(int target_x, int target_y, int img_bar_length, double currentPitch, double currentYaw)
{
    Point2d offset;
    offset.x = target_x - img_width / 2;
    offset.y = -target_y + img_height / 2;

    Vector3d tempPos1;
    tempPos1.x = offset.x * bar_length / img_bar_length;
    tempPos1.y = focal_length * bar_length / img_bar_length;
    tempPos1.z = offset.y * bar_length / img_bar_length;

    // 需要旋转一下这个矩阵
    // 绕pitch轴旋转
    Vector3d tempPos2;
    tempPos2.x = tempPos1.x;
    tempPos2.y = cos(currentPitch) * tempPos1.y - sin(currentPitch) * tempPos1.z;
    tempPos2.z = sin(currentPitch) * tempPos1.y + cos(currentPitch) * tempPos1.z;
    // 绕yaw轴旋转
    this->enemy_pos.x = (cos(currentYaw) * tempPos2.x - sin(currentYaw) * tempPos2.y) / 1000;
    this->enemy_pos.y = (sin(currentYaw) * tempPos2.x + cos(currentYaw) * tempPos2.y) / 1000;
    this->enemy_pos.z = tempPos2.z / 1000;
    // enemy_pos = pos;
}
void angleSolver::setEnemyVelocity(Point3f velocity)
{
    enemy_v = velocity;
}

void angleSolver::init(int img_width, int img_height, double focal_length, double bar_length)
{
    this->img_width = img_width;
    this->img_height = img_height;
    this->focal_length = focal_length;
    this->bar_length = bar_length;
}

////////////////////////////////////////////////

void angleSolver::get_Position(Armor &armor)
{
    Point2f p1;
    p1.x = armor.center.x - armor.distance / 2;
    p1.y = armor.center.y + armor.barLen / 2;
    Point2f p2;
    p2.x = armor.center.x + armor.distance / 2;
    p2.y = armor.center.y + armor.barLen / 2;
    Point2f p3;
    p3.x = armor.center.x + armor.distance / 2;
    p3.y = armor.center.y - armor.barLen / 2;
    Point2f p4;
    p4.x = armor.center.x - armor.distance / 2;
    p4.y = armor.center.y - armor.barLen / 2;
    //*********For the first bar**********
    float D1 = 509.f * BAR_REAL_LENGTH / abs(p1.y - p4.y);
    float X1 = BAR_REAL_LENGTH / abs(p1.y - p4.y) * p1.x;
    float Z1 = BAR_REAL_LENGTH / abs(p1.y - p4.y) * ((p1.y + p4.y) / 2);
    //*********For the second bar**********
    float D2 = 509.f * BAR_REAL_LENGTH / abs(p2.y - p3.y);
    float X2 = BAR_REAL_LENGTH / abs(p2.y - p3.y) * p2.x;
    float Z2 = BAR_REAL_LENGTH / abs(p2.y - p3.y) * ((p2.y + p3.y) / 2);

    armor.position.x = (X1 + X2) / 2;
    armor.position.y = (D1 + D2) / 2;
    armor.position.z = (Z1 + Z2) / 2;
}

double angleSolver::get_para_t(double vx0, double dis, double alpha)
{
    dis -= L * cos(alpha);
    double t = log(vx0 / (vx0 - (Co * dis))) / Co;
    return t;
}

double angleSolver::get_para_height(double vx0, double vy0, double dis, double alpha)
{
    double t = get_para_t(vx0, dis, alpha);
    double y = (-g / Co) * t + (vy0 + g / Co) / Co * (1 - exp(-Co * t));
    return y;
}

double angleSolver::get_str_v(double alpha)
{
    double v = sqrt(V * V - 2 * Miu * g * L * cos(alpha) - 2 * g * L * sin(alpha));
    return v;
}

double angleSolver::get_height(double alpha, double dis)
{
    double v = get_str_v(alpha);
    double vx0 = v * cos(alpha);
    double vy0 = v * sin(alpha);
    double y = get_para_height(vx0, vy0, dis, alpha);
    return y + H + L * sin(alpha);
}

double angleSolver::select_alpha(double dis)
{
    double alpha = 0, alpha_final = 0;
    double delta_final = 999, delta = 0;
    for (int i = 0; i < 180; i++)
    {
        alpha = alpha + 0.25 / 180 * M_PI;
        double height = get_height(alpha, dis);
        delta = abs(height - Z);
        if (delta < delta_final)
        {
            delta_final = delta;
            alpha_final = alpha;
        }
    }

    return alpha_final;
}

double angleSolver::get_thet(double X, double Y, double X_velocity, double Y_velocity, double thet0)
{
    double delta_thet = 0;
    double atanYoverX = 0;
    if (X == 0)
    {
        if (Y > 0)
            atanYoverX = M_PI_2;
        if (Y < 0)
            atanYoverX = -M_PI_2;
        if (Y == 0)
            ; //Y&&X can't be 0 at a same time
    }
    else
    {
        atanYoverX = atan(Y / X);
    }

    double atanYVoverXV = 0;
    if (X_velocity == 0)
    {
        if (Y_velocity > 0)
            atanYVoverXV = M_PI_2;
        if (Y_velocity < 0)
            atanYVoverXV = -M_PI_2;
        if (Y_velocity == 0)
            ;
    }
    else
    {
        atanYVoverXV = atan(Y_velocity / X_velocity);
    }

    double phi = M_PI - atanYoverX + atanYVoverXV;
    double V_object = sqrt(X_velocity * X_velocity + Y_velocity * Y_velocity);
    if (X_velocity == 0 && Y_velocity == 0)
    {
        delta_thet = 0;
    }
    else
    {
        delta_thet = asin(V_object * sin(phi) / V);
    }
    return thet0 + delta_thet;
}

double angleSolver::get_ALPHA()
{
    return angleSolver::select_alpha(dis);
}

double angleSolver::get_THET()
{
    double thet = 0;
    if (Y != 0)
    {
        thet = atan(X / Y);
    }
    else
    {
        thet = M_PI_2;
    }
    return angleSolver::get_thet(X, Y, X_velocity, Y_velocity, thet);
}

void angleSolver::setXYZDis(double tempX, double tempY, double tempZ)
{
    X = tempX;
    Y = tempY;
    Z = tempZ;
    dis = sqrt(X * X + Y * Y + Z * Z) / 100;
}

void angleSolver::setRelativeV(double Vx, double Vy)
{
    X_velocity = Vx; // to the right
    Y_velocity = Vy; // to the front
}

void angleSolver::setBulletSpeed(double bullet_speed)
{
    this->bullet_v = bullet_speed;
}

double angleSolver::getDis()
{
    return sqrt(X * X + Y * Y + Z * Z);
}
